#ifndef _ASTART_SEARCHER_H
#define _ASTART_SEARCHER_H

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <Eigen/Eigen>
#include "backward.hpp"
#include "node.h"

// A* Search 的父类
class AstarPathFinder
{	
	private:

	protected:
		uint8_t * data;
		GridNodePtr *** GridNodeMap;
		Eigen::Vector3i goalIdx;
		int GLX_SIZE, GLY_SIZE, GLZ_SIZE;
		int GLXYZ_SIZE, GLYZ_SIZE;

		double resolution, inv_resolution;
		double gl_xl, gl_yl, gl_zl;
		double gl_xu, gl_yu, gl_zu;

		// getHeu方法使用的算法,0:Euclidean Distance,1;曼哈顿距离算法
		int heu_type = 0;

		// 终止节点指针，目标位置 ？ 
		GridNodePtr terminatePtr;
		// 展开过的节点数据集，open set 实现:用C++ STL 中的 multimap 实现
		// 用C++ STL中的multimap实现,multimap将{key,value}当做元素,允许重复元素。multimap根据key的排序准则自动将元素排序,因此使用时只需考虑插入和删除操作即可。
		std::multimap<double, GridNodePtr> openSet;

		// 启发式函数，作业完成
		double getHeu(GridNodePtr node1, GridNodePtr node2);
		// 拓展节点函数，作业完成 
		void AstarGetSucc(GridNodePtr currentPtr, std::vector<GridNodePtr> & neighborPtrSets, std::vector<double> & edgeCostSets);		

    	bool isOccupied(const int & idx_x, const int & idx_y, const int & idx_z) const;
		bool isOccupied(const Eigen::Vector3i & index) const;
		bool isFree(const int & idx_x, const int & idx_y, const int & idx_z) const;
		bool isFree(const Eigen::Vector3i & index) const;
		
		Eigen::Vector3d gridIndex2coord(const Eigen::Vector3i & index);
		Eigen::Vector3i coord2gridIndex(const Eigen::Vector3d & pt);
		
		GridNodePtr & TieBreaker(const std::multimap<double, GridNodePtr> &  openSet, const GridNodePtr & endPtr);
	public:
		AstarPathFinder(){};
		~AstarPathFinder(){};
		// A*搜索算法函数,作业完成
		void AstarGraphSearch(Eigen::Vector3d start_pt, Eigen::Vector3d end_pt);
		void resetGrid(GridNodePtr ptr);
		void resetUsedGrids();

		// 设置getHeu方法使用的算法,默认为 0:Euclidean Distance,1;曼哈顿距离算法
		void setHeuType(int type) {
			heu_type = type;
		}

		void initGridMap(double _resolution, Eigen::Vector3d global_xyz_l, Eigen::Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id);
		void setObs(const double coord_x, const double coord_y, const double coord_z);

		Eigen::Vector3d coordRounding(const Eigen::Vector3d & coord);
		std::vector<Eigen::Vector3d> getPath();
		std::vector<Eigen::Vector3d> getVisitedNodes();
};

#endif